#include <points.hpp>
namespace ohe_framework
{

Point::Point()
{
  
}
Point::Point(unsigned short x_value,unsigned short y_value)
{
	x = x_value;
	y = y_value;
}
Point::~Point()
{

}


Blob::Blob()
{
    no_of_points=0;
    rectangle_found = false;
    center_calculated = false;
}
Blob::~Blob()
{

}

int Blob::getNoOfPoints()
{
	return no_of_points;
}

void Blob::addPoint(Point point)
{
	points.push_back(point);
        rectangle_found = false;
        center_calculated = false;
	no_of_points++;
}

bool Blob::addPoint(Point point,const int index)
{
    if(index<no_of_points)
	{
		points.insert(points.begin()+index,point);
	}
	else if(index==(no_of_points+1))
		{
			points.push_back(point);
		}
		else
		{
		  printErrorMessage("Index is invalid");
		  return false;
		}
    rectangle_found = false;
    center_calculated = false;
	no_of_points++;
	return true;
}

bool Blob::removePoint(int index=-1)
{
	if((index==-1)&&(!points.empty()))
	{
		points.pop_back();
                rectangle_found = false;
                center_calculated = false;
		no_of_points--;
		return true;
	}
	if(index<no_of_points)
	{
		points.erase(points.begin()+index);
		no_of_points--;
	}
	else
	{
		printErrorMessage("Index is invalid");
	    return false;
	}
        rectangle_found = false;
        center_calculated = false;
	return true;
}

bool Blob::removePoint(Point& point)
{
	if(!points.empty())
	{
		for(int i=0;i<no_of_points;i++)
		{
			if((points[i].x==point.x)&&(points[i].y==point.y))
			{
				points.erase(points.begin()+i);
				no_of_points--;
                                rectangle_found = false;
                                center_calculated = false;
				return true;
			}
		}
	}
	return false;
}

void Blob::getCenter(Point &_center)
{
    if(!center_calculated)
    {
        unsigned long  x_center=0, y_center=0;
        Point _start,_end;
        if(!rectangle_found)
            getBoundingRectangle(_start,_end);
        unsigned int x_devide=1,y_devide=1;
        for(int j =start.y;j<=end.y;j++)
        {
            int x_min = end.x;
            int x_max = start.x;
            for(int i=0;i<no_of_points;i++)
            {
                if(points[i].y==j)
                {
                    if(points[i].x>x_max)
                    {
                        x_max =points[i].x; 
                    }
                    if(points[i].x<x_min)
                    {
                        x_min =points[i].x; 
                    }
                }
            }
            y_center+=(x_max-x_min)*(j-start.y);
            y_devide+=(j-start.y);
        }
        for(int j =start.x;j<=end.x;j++)
        {
            int y_min = end.y;
            int y_max = start.y;
            for(int i=0;i<no_of_points;i++)
            {
                if(points[i].x==j)
                {
                    if(points[i].y>y_max)
                    {
                        y_max =points[i].y; 
                    }
                    if(points[i].y<y_min)
                    {
                        y_min =points[i].y; 
                    }
                }
            }
            x_center+=(y_max-y_min)*(j-start.x);
            x_devide+=(j-start.x);
        }
        center.x = start.x+(x_center/x_devide);
        center.y = start.y+(y_center/y_devide);
        center_calculated = true;
    }
    _center = center;
}

void Blob::getBoundingRectangle(Point& _start,Point& _end)
{
    if(!rectangle_found)
    {
        unsigned int min_x,min_y,max_x,max_y;
        min_x=max_x=points[0].x;
        min_y=max_y=points[0].y;
        for(int i=1;i<no_of_points;i++)
        {
            if(points[i].x>max_x)
            {
                max_x = points[i].x;
            }
            if(points[i].x<min_x)
            {
                min_x = points[i].x;
            }
            if(points[i].y>max_y)
            {
                max_y = points[i].y;
            }
            if(points[i].y<min_y)
            {
                min_y = points[i].y;
            }
        }
        start.x = min_x;
        start.y = min_y;
        end.x = max_x;
        end.y = max_y;
        rectangle_found = true;
    }
    _start = start;
    _end = end;
}

std::vector<Point>* Blob::getPoints()
{
    return &points;
}

CircularBufferPoint::CircularBufferPoint(int slot)
{
    if(slot<=0)
    {
        no_of_slots = 5;
    }
    else
    {
        no_of_slots = slot;
    }
    uppers =0;
    lowers = 0;
    radius=0;
    clear();
}

bool CircularBufferPoint::write(Point &upper, Point &lower, Point &_radius)
{
    if(upper.y>lower.y)
    {
        uppers[write_index] = upper;
        lowers[write_index] = lower;
    }
    else
    {
        uppers[write_index] = lower;
        lowers[write_index] = upper;
    }
//    if(read_index == -1)
//    {
//        read_index = write_index;
//    }
    radius[write_index]=_radius;
    write_index = (write_index+1)%no_of_slots;
    return true;
}

bool CircularBufferPoint::read(int index, Point &upper, Point &lower, Point &_radius)
{
    index = (index+no_of_slots)%no_of_slots;
    upper = uppers[index];
    lower = lowers[index];
    _radius = radius[index];
    return true;
}

int CircularBufferPoint::getWriteIndex()
{
    return (write_index+no_of_slots-1)%no_of_slots;
}

void CircularBufferPoint::clear()
{
    read_index = -1;
    write_index = 0;
    if(uppers!=0)
        delete[] uppers;
    if(lowers!=0)
        delete[] lowers;
    if(radius!=0)
        delete[] radius;
    uppers = new Point[no_of_slots];
    lowers = new Point[no_of_slots];
    radius = new Point[no_of_slots];
    for(int i =0; i<no_of_slots;i++)
    {
        uppers->x = uppers->y = 0;
        lowers->x = lowers->y = 0;
        radius->x = radius->y = 0;
    }
}

CircularBufferPoint::~CircularBufferPoint()
{
    delete[] uppers;
    delete[] lowers;
    delete[] radius;
}
}
